#include "zf_common_headfile.h"

uint8 OLED_Count = 255;
//uint8 var[8];
float OutData[4];
extern EulerAngleTypedef       SystemAttitude;            /////��̬��
extern BMX055Datatypedef       BMX055Data;
//extern EulerAngleTypedef       SystemAttitude;
//extern EulerAngleTypedef       SystemAttitudeRate;
//extern uint16 ad_value[6];    //�ĸ����ADֵ�洢
//extern AttitudeDatatypedef     GyroOffset;
extern float lboacc[3];
//extern	int left_k_63,left_k_65,left_k_69,right_k_63,right_k_65,right_k_69;

/* VOFA���ݻ������� */
float VOFA_debug[24] = {0};
//uint8 ch='s';

/*  @�������֣�void User_DeBug(void)
 *  @���ߣ�����
 *  @�������ã���������ʾҳ��
 *  @����ֵ����
 *  @��������
 *  @ʱ�䣺2023��11��21��
 * */
extern float errrrr;
extern int left[6];
extern int right[6];
float dj_pwm = 0;
void User_DeBug(void)
{
    switch(Balance_Test)
    {
        case 0://ƫ�������ʾ
            oled_show_float(0, 0, angle_value, 4, 4);
            oled_show_float(0, 1, Speed_out_left, 8, 4);
            oled_show_float(0, 2, Speed_out_right, 8, 4);
            oled_show_float(0, 4, Speed_P, 4, 4);
            oled_show_float(0, 5, Speed_I, 4, 4);
            oled_show_float(0, 6, Speed_D, 4, 4);
            oled_show_float(60, 7, Encoder_LeftSpeed, 4, 2);
            oled_show_float(0, 7, errrrr, 4, 2);
            break;
        case 1://������������ʾ
            oled_show_string(7, 0, "Encoder");
            oled_show_string(0, 1, "EnSu:");
            oled_show_float(30, 1, Encoder_RawSpeedSum, 6, 3);
            oled_show_string(0, 2, "EnSp:");
            oled_show_float(30, 2, Encoder_LeftSpeed, 4, 2);
            oled_show_float(80, 2, Encoder_RightSpeed, 4, 2);
            oled_show_string(0, 3, "EnL_Count:");
            oled_show_int(0, 4, Right_EncoderCount,8);
            oled_show_string(0, 5, "EnR_Count:");
            oled_show_int(0, 6, Left_EncoderCount,8);
            oled_show_string(0, 7, "CSum:");
            oled_show_int(30, 7, EncoderCountSum,8);
            if(blue_test==1)
            {
                VOFA_debug[0] = Encoder_RawSpeedSum;//����
                VOFA_debug[1] = Encoder_LeftSpeed;//������
                VOFA_debug[2] = Encoder_RightSpeed;//������
                VOFA_debug[3] = Speed_P;//�ٶȻ�P
                VOFA_debug[4] = Speed_I;//�ٶȻ�I
                VOFA_debug[5] = Speed_D;//�ٶȻ�D
                VOFA_debug[6] = Speed_out_left;//�����ٶȻ����
                VOFA_debug[7] = Speed_out_right;//�����ٶȻ����
                VOFA_debug[8] = Speed_Aim;//�����ٶ�
                sendCamImgToVOFA(VOFA_debug);//��������
            }
            break;
        case 3://ƫ�������ʾ
            oled_show_string(4, 0, "ShengXiangTO");
            oled_show_float(0, 1, angle_value, 4, 4);
            oled_show_float(0, 2, Exp_Time, 8, 4);
            oled_show_float(60, 2, Gain, 8, 4);
            oled_show_float(0, 3, value[0], 4, 4);
            oled_show_float(30, 3, value[1], 4, 4);
            oled_show_float(60, 3, value[2], 4, 4);

            oled_show_float(0, 4, value[3], 4, 2);
            oled_show_float(30, 4, value[4], 4, 2);
            oled_show_float(60, 4, value[5], 4, 2);

//            oled_show_uint(0, 5, zhizou_jilu, 3);
//            oled_show_uint(30, 5, zhizou_zuobian, 3);
//            oled_show_uint(60, 5, edge_Width[3], 3);
//            oled_show_uint(80, 5, edge_Width[4], 3);
//            oled_show_uint(100, 5, edge_Width[5], 3);
//
//            oled_show_int(0, 6, right[0], 3);
//            oled_show_int(20, 6, right[1], 3);
//            oled_show_int(40, 6, right[2], 3);
//            oled_show_int(60, 6, right[3], 3);
//            oled_show_int(80, 6, right[4], 3);
//            oled_show_int(100, 6, right[5], 3);
//
//            oled_show_uint(0, 7, left[0], 3);
//            oled_show_uint(20, 7, left[1], 3);
//            oled_show_uint(40, 7, left[2], 3);
//            oled_show_uint(60, 7, left[3], 3);
//            oled_show_uint(80, 7, left[4], 3);
//            oled_show_uint(100, 7, left[5], 3);
            if(blue_test==2)
            {
                sendCamImgToCamViewer();
            }

            break;
        case 4://ƫ�������ʾ
            oled_show_string(4, 0, "TuoLuoyi");
            oled_show_float(0, 0, angle_value, 4, 4);
            oled_show_float(0, 1, new_gyro_z, 4, 4);
            oled_show_float(0, 2, Att_Angle.yaw, 4, 4);

            VOFA_debug[0] = new_gyro_z;//������
            VOFA_debug[1] = angle_value;//ƫ��
            sendCamImgToVOFA(VOFA_debug);//��������
            break;
        case 5://ƫ�������ʾ
            oled_show_string(4, 0, "DUOJI");
            pit_disable(CCU60_CH0);
            while(Key_Read()==4)
                dj_pwm ++ ;
            while(Key_Read()==3)
                dj_pwm --;
            oled_show_float(0, 1, dj_pwm, 4, 4);
            Servo_Out(dj_pwm);
            break;
        default :
            break;
    }
}


void OutPut_Data (void)
{
    int temp[4] = {0};
    unsigned int temp1[4] = {0};
    unsigned char databuf[10] = {0};
    unsigned char i;
    unsigned short CRC16 = 0;
    for (i = 0; i < 4; i++)
    {

        temp[i] = (int16) OutData[i];
        temp1[i] = (uint16) temp[i];

    }

    for (i = 0; i < 4; i++)
    {
        databuf[i * 2] = (int8) (temp1[i] % 256);
        databuf[i * 2 + 1] = (int8) (temp1[i] / 256);
    }

//    CRC16 = CRC_CHECK(databuf, 8);
    databuf[8] = CRC16 % 256;
    databuf[9] = CRC16 / 256;

    for (i = 0; i < 10; i++)
        uart_write_byte(UART_1, (char) databuf[i]);
}

